package robocup.component.worldmodel;

import java.util.Arrays;
import java.util.Collections;
import java.util.EnumMap;
import java.util.LinkedList;

import robocup.component.DAInamiteConstants;
import robocup.component.PConf;
import robocup.component.SConf;
import robocup.component.WorldModel;
import robocup.component.ControllerNumbers.PLAY_MODE;
import robocup.component.ControllerNumbers.TEAM;
import robocup.component.actions.DashAction;
import robocup.component.geometry.Vektor;
import robocup.component.infotypes.CoachSayInfo;
import robocup.component.infotypes.InfoReceiver;
import robocup.component.infotypes.PlayModeInfo;
import robocup.component.infotypes.PlayerInfo;
import robocup.component.infotypes.PlayerSayInfo;
import robocup.component.infotypes.PlayerTypesInfo;
import robocup.component.infotypes.RefereeInfo;
import robocup.component.infotypes.SenseBodyInfo;
import robocup.component.infotypes.VisualInfo;
import robocup.component.math.PermutationEvaluator;
import robocup.component.math.RCMath;
import robocup.component.speechacts.MessageFactory;
import robocup.component.speechacts.MessageFactory.MESSAGES;
import robocup.component.tactics.AbstractState;
import robocup.component.tactics.DefaultPositions;
import robocup.component.tactics.AbstractState.STATES;
import robocup.component.util.comparators.PlayerComparator;
import robocup.component.util.comparators.PlayerComparator.DIMENSION;
import robocup.component.util.comparators.PlayerComparator.ORDER;
import robocup.component.worldobjects.Ball;
import robocup.component.worldobjects.Player;
import robocup.component.worldobjects.Player.UPDATE_MODE;

/**
 * PlayersModel is the part of the WorldModel which implements functions to
 * calculate the state of the other players (position, speed, direction, ...).
 * 
 * There exists 3 player types in this model: <br>
 * <br>
 * <b>normal players</b> - these players are based only on certain information
 * (visual infos, say infos) ({@link #allPlayers}, {@link #teammates},
 * {@link #opponents})<br>
 * <b>expected players</b> - these players are updated by visual and say infos
 * and by predicting these players actions ({@link #expectedPlayers},
 * {@link #expectedTeammates} , {@link #expectedOpponents})<br>
 * <b>next players</b> - same as the <code>expected players</code> but we
 * predict the action of the current cycle as well. These players are especially
 * useful for pass caluclations and for the anticipation of the opponent's
 * behaviour. ({@link #nextPlayers})
 * 
 * Players will be updated when they were seen, communicated or their action
 * could be predicted. See the corresponding methods for details.
 * 
 * TODO
 *  - put all matching stuff to the {@link PermutationEvaluator}.
 */
public class PlayersModel
    implements InfoReceiver {

  /**
   * the dominated side of the field.
   * 
   * this is used for indicating what side most of the players are located on.
   * It is used for telling if passes to change the side would be useful.
   * 
   * for the values it is assumed that the opponents goal is in front of us thus
   * LEFT meaning our left wing.
   */
  public enum DOMINATED_SIDE {
    LEFT,
    NONE,
    RIGHT
  }

  /**
   * use this for complex predefined sorting.
   */
  public enum SORTED {
    EXPECTED_OPPONENTS_BY_ANGLE_ASC(PlayerComparator.DIMENSION.ANGLE, PlayerComparator.ORDER.ASCENDING),
    EXPECTED_OPPONENTS_BY_X_ASC(PlayerComparator.DIMENSION.X_AXIS, PlayerComparator.ORDER.ASCENDING),
    EXPECTED_OPPONENTS_BY_Y_ASC(PlayerComparator.DIMENSION.Y_AXIS, PlayerComparator.ORDER.ASCENDING),
    EXPECTED_OPPONENTS_BY_DISTANCE_ASC(PlayerComparator.DIMENSION.DISTANCE, PlayerComparator.ORDER.ASCENDING),
    EXPECTED_TEAMMATES_BY_DISTANCE_ASC(PlayerComparator.DIMENSION.DISTANCE, PlayerComparator.ORDER.ASCENDING),
    EXPECTED_TEAMMATES_BY_Y_ASC(PlayerComparator.DIMENSION.Y_AXIS, PlayerComparator.ORDER.ASCENDING),
    EXPECTED_TEAMMATES_BY_X_ASC(PlayerComparator.DIMENSION.X_AXIS, PlayerComparator.ORDER.ASCENDING);

    // the attribute/dimension to order by
    private DIMENSION dimension;

    // asc/desc
    private ORDER     order;

    // last time ordering occured
    private int       lastSortingTime = -1;

    /**
     * Constructor.
     * 
     * @param dimension -
     *          the dimension/attribute to order by
     * @param order -
     *          asc or desc
     */
    SORTED(
        final PlayerComparator.DIMENSION dimension,
        final PlayerComparator.ORDER order) {

      this.dimension = dimension;
      this.order = order;
    }

    /**
     * @return Returns the order.
     */
    public ORDER getOrder() {

      return this.order;
    }

    /**
     * @return Returns the dimension.
     */
    public DIMENSION getDimension() {

      return this.dimension;
    }

    /**
     * @return Returns the lastSortingTime.
     */
    public int getLastSortingTime() {

      return this.lastSortingTime;
    }

    /**
     * @param lastSortingTime
     *          The lastSortingTime to set.
     */
    public void setLastSortingTime(final int lastSortingTime) {

      this.lastSortingTime = lastSortingTime;
    }

  }

  // the world for other worldmodel parts methods calls
  private final WorldModel                world;

  // PlayerBean
  private final SConf                     sConf                     = SConf
                                                                  .getInstance();                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        // Server

  // the teammates (based only on certain knowledge)
  private final Player[]                  teammates                 = new Player[11];

  // the opponents (based only on certain knowledge)
  private final Player[]                  opponents                 = new Player[11];

  /**
   * {@link #allPlayers} = [0..10]{@link #teammates} + [11..21]{@link #opponents}
   * (based only on certain knowledge)<br>
   * 
   * the players in {@link #allPlayers} and the players in {@link #teammates}/
   * {@link #opponents} are the same instances!!! This way it is possible to
   * return only subsets of all players.
   */
  private final Player[]                  allPlayers                = new Player[22];

  // the teammates (based on certain knowledge + predictions)
  private final Player[]                  expectedTeammates         = new Player[11];

  // the opponents (based on certain knowledge + predictions)
  private final Player[]                  expectedOpponents         = new Player[11];

  /**
   * {@link #expectedPlayers} = [0..10]{@link #expectedTeammates} + [11..21]{@link #expectedOpponents}<br>
   * 
   * expectedPlayers = {@link #allPlayers} + predicted actions for all cycles
   * with no certain knowledge<br>
   * 
   * the players in {@link #expectedPlayers} and the players in
   * {@link #expectedTeammates}/{@link #expectedOpponents} are the same
   * instances!!! This way it is possible to return only subsets of expected
   * players.
   */
  private final Player[]                  expectedPlayers           = new Player[22];

  /**
   * {@link #nextPlayers} = {@link #expectedPlayers} + this cycles predicted
   * actions
   */
  private final Player[]                  nextPlayers               = new Player[22];

  // Buffer for less Vektor creation
  private final Vektor                            newPlayerPosAbs           = new Vektor();

  // Buffer for less Vektor creation
  private final Vektor                            newPlayerSpeedAbs         = new Vektor();

  // We have to update players with full information first.
  // So we need some buffers...
  private final LinkedList<PlayerInfo>            teammatesWithoutNumber    = new LinkedList<PlayerInfo>();

  private final LinkedList<PlayerInfo>            opponentsWithoutNumber    = new LinkedList<PlayerInfo>();

  private final LinkedList<PlayerInfo>            playersWithoutTeam        = new LinkedList<PlayerInfo>();

  private final LinkedList<PlayerInfo>            playersWithAll            = new LinkedList<PlayerInfo>();

  // the return list for the matching method
  private final LinkedList<Player>        matchedPlayersBuffer      = new LinkedList<Player>();

  /**
   * #matchDistances[info][player] - This is a distance matrix. An entry in this
   * matrix represents the distance between a seen player and the expected
   * position of another. Values < 0 mean that the player had no time to reach
   * this position and this combination of player X info is excluded. The actual
   * size of the matrix may vary as the number of seen player's and candidates
   * varies.
   * 
   * @see #update(VisualInfo),
   * @see #matchDistancesColumnCount
   * @see #matchDistancesRowCount
   * 
   * The max. dimension of the matrix would be 21x21 as the player itself is
   * excluded. However, the first row contains the indexes of the players used.
   * 
   */
  private final double[][]                 matchDistances            = new double[22][21];

  // number of columns filled in the match matrix
  private int                        matchDistancesColumnCount = -1;

  // number of rows filled in the match matrix
  private int                        matchDistancesRowCount    = -1;

  // the matching itself is is O(n!).
  private final  double                     maxMatchDistance          = 30.0;

  // the value indicating that the matrix entry is empty
  private final  double                     emptyValue                = -1.0;

  // the opponents goalie number
  private int                       numberGoalieOpponent      = -1;

  // the matching itself is is O(n!) so we limit the number of iterations
  // this should be sufficient as the matrix should have many holes
  private final int                       matchCounterLimit         = 30;

  private final Vektor                    dummyVektor               = Vektor
                                                                  .getDummyInstance();

  private final Vektor                    dummyVektor2              = Vektor
                                                                  .getDummyInstance();

  private final DashAction                dash100                   = new DashAction(
                                                                  100);

  // turn on to get debug output
  private final boolean                   DEBUG                     = false;

  // the player to print the debug output
  private final int                       DEBUG_PLAYER              = 10;

  // a dummy player comparator
  private final PlayerComparator          playerComparator          = new PlayerComparator(
                                                                  DIMENSION.X_AXIS);

  // this map contains a number of sorted player arrays
  private final EnumMap<SORTED, Player[]> sortedPlayersMemory       = new EnumMap<SORTED, Player[]>(
                                                                  SORTED.class);

  // the last times a sorting occured
  private final EnumMap<SORTED, Integer>  sortingTimes              = new EnumMap<SORTED, Integer>(
                                                                  SORTED.class);

  // the dominated side
  private DOMINATED_SIDE            dominatedSide             = DOMINATED_SIDE.NONE;

  // true if two players are neighbours for all player combinations
  private final boolean[][]               neighbours                = new boolean[][] { { false, true, true, true, true, false, false, false, false, false, false }, { true, false, true, false, false, true, true, false, false, false, false }, { true, true, false, true, false, true, true, false, false, false, false }, { true, false, true, false, true, false, true, true, false, false, false }, { true, false, false, true, false, false, true, true, false, false, false }, { false, true, true, false, false, false, true, false, true, true, false }, { false, true, true, true, true, true, false, true, true, true, true }, { false, false, false, true, true, false, true, true, false, true, true }, { false, false, false, false, false, true, true, false, false, true, false }, { false, false, false, false, false, true, true, true, true, false, true }, { false, false, false, false, false, false, true, true, false, true, false } };

  // true if two players are neighbours for all player combinations
  private final boolean[][]               directNeighbours                = new boolean[][] { { false, true, true, true, true, false, false, false, false, false, false }, { true, false, true, false, false, true, false, false, false, false, false }, { true, true, false, true, false, true, true, false, false, false, false }, { true, false, true, false, true, false, true, true, false, false, false }, { true, false, false, true, false, false, false, true, false, false, false }, { false, true, false, false, false, false, true, false, true, false, false }, { false, false, true, true, false, true, false, true, false, true, false }, { false, false, false, false, true, false, true, false, false, false, true }, { false, false, false, false, false, true, false, false, false, true, false }, { false, false, false, false, false, false, true, false, true, false, true }, { false, false, false, false, false, false, false, true, false, true, false } };

  /**
   * Constructor for PlayersModel - this model stores all information referring
   * to other players (teammates and opponents) and provides the update logic
   * and some useful functions.
   * 
   * @param world -
   *          the current world-model (parent)
   */
  public PlayersModel(
      final WorldModel world) {

    this.world = world;
    this.initPlayerArrays();

    // create sorting map once
    for (final SORTED e : SORTED.values()) {
      switch (e) {
        case EXPECTED_OPPONENTS_BY_ANGLE_ASC:
        case EXPECTED_OPPONENTS_BY_X_ASC:
        case EXPECTED_OPPONENTS_BY_DISTANCE_ASC:
        case EXPECTED_OPPONENTS_BY_Y_ASC:
          this.sortedPlayersMemory.put(e, this.expectedOpponents);
          break;
        case EXPECTED_TEAMMATES_BY_DISTANCE_ASC:
        case EXPECTED_TEAMMATES_BY_X_ASC:
        case EXPECTED_TEAMMATES_BY_Y_ASC:
          this.sortedPlayersMemory.put(e, this.expectedTeammates);
          break;

        default:
      }
    }
  }

  /**
   * <code>{@link #update(SenseBodyInfo)}</code> extrapolates players based on
   * predictions about their performed actions
   * 
   * @param info -
   *          {@link SenseBodyInfo} for the update
   * 
   */
  public void update(final SenseBodyInfo info) {

    if (this.world.getPlayMode() == PLAY_MODE.KICK_OFF_OWN) {
      for (final Player p : this.teammates) {
        p.setBodyCycle(this.world.getBodyCycle());
      }
    }

    // extrapolate predicted players based on the predicted action
    for (int i = 0; i < this.allPlayers.length; i++) {
      if (!this.allPlayers[i].exists()) {
        continue;
      }

      // we know the self from the bs-info so we don't have to predict it
      // again. We reset players that are not reliable anymore to there
      // last known position
      if (this.allPlayers[i].isMe() || !this.expectedPlayers[i].isReliable()) {
        this.expectedPlayers[i].copy(this.allPlayers[i]);
        continue;
      }
      // we predicted an action for a player so the predicted player of
      // this cycle is overwritten by the nextPlayer of the last one
      else if (this.nextPlayers[i].isPredicted()) {
        this.expectedPlayers[i].copy(this.nextPlayers[i]);
        this.expectedPlayers[i].setBodyCycle(this.world.getBodyCycle());
        this.expectedPlayers[i].setCycle(this.world.getCycle());
        this.expectedPlayers[i].setUpdateMode(UPDATE_MODE.EXPECTED, this.world
            .getBodyCycle());
      }
      // no prediction -> we extrapolate only
      else if (i < 11) {
        this.expectedPlayers[i].predictThisAfterAction(null);
      }
    }

    // tackle dead counter decreases by one for tackle dead players
    for (int i = 0; i < this.allPlayers.length; i++) {
      if (this.allPlayers[i].isMe()) {
        continue;
      }

      this.allPlayers[i].setTackledeadCountdown(Math.max(this.allPlayers[i]
          .getTackledeadCountdown() - 1, 0));
      this.expectedPlayers[i].setTackledeadCountdown(this.allPlayers[i]
          .getTackledeadCountdown());
    }

    // decide which players were offside in the last cycle and will not
    // intercept the kicked ball
    if (this.world.getSelfRef().getLastState().isKickState()) {
      this.setPlayersOffside(this.world.getNumber() - 1);
    }

    // this.predictNextPlayers();

  }

  /**
   * <code>updateVisual</code> updates all seen players
   * 
   * Sometimes we see players without their number so we must decide which
   * player it was we saw. This class' matching logic is based on the least
   * square algorithm as it tries to find out what combination of playersxinfos
   * will have the lowest sum of squared deviation.<br>
   * <br>
   * Matching is done in 4 steps:<br>
   * <br>
   * <b>1.</b> All Players with their number seen are matched.<br>
   * <b>2.</b> All Teammates are matched (players matched in 1. excluded).<br>
   * <b>3.</b> All Opponents are matched (players matched in 1. excluded).<br>
   * <b>4.</b> All Players without teaminfo and number are matched (players
   * matched in 1., 2. or 3. excluded)<br>
   */
  public void update(final VisualInfo info) {

    // do not update players for 5 cycles, if the ball is too far away,
    // and we are not midfield-Players (by number)
    // this is only done to save time (delete if you have more ressources)
    if (this.world.getNumber() != 6 && this.world.getNumber() != 7 && this.world
        .getNumber() != 8 && (this.world.getBodyCycle() + this.world
        .getNumber()) % 5 != 0 && this.world.getDistance(this.world
        .getBallRef()) > 50 && this.world.getPlayMode() == PLAY_MODE.PLAY_ON) {
      return;
    }

    // do not update players for 2 cycles, if the ball is too far away,
    // and we are midfield-Players (by number)
    // this is only done to save time (delete if you have more ressources)
    if ((this.world.getNumber() == 6 || this.world.getNumber() == 7 || this.world
        .getNumber() == 8) && (this.world.getBodyCycle() + this.world
        .getNumber()) % 2 != 0 && this.world.getDistance(this.world
        .getBallRef()) > 50 && this.world.getPlayMode() == PLAY_MODE.PLAY_ON) {
      return;
    }

    // reset buffers
    this.teammatesWithoutNumber.clear();
    this.opponentsWithoutNumber.clear();
    this.playersWithoutTeam.clear();
    this.playersWithAll.clear();

    // get all seen players
    final PlayerInfo[] pInfos = info.getPlayers();

    // fill buffers
    for (final PlayerInfo pi : pInfos) {
      if (this.world.getOpponentTeamName().length() == 0) {
        if (!pi.isFriend() && !pi.getTeamName().contains("unknownTeam")) {
          this.world.setOpponentTeamName(pi.getTeamName());
        }
      }

      // the absolute position of the seen player
      final Vektor posAbs = new Vektor(pi.getDistance(),
          pi.getDirection() + this.world.getSelfRef().getHeadDir());
      posAbs.addToThis(this.world.getSelfRef().getPosition());

      pi.setAbsPlayerPosition(posAbs);

      // split the infos depending on their type
      if (pi.isOfUnknownTeam()) {
        this.playersWithoutTeam.add(pi);
      }
      else if (pi.getNumber() < 0) {
        if (pi.isFriend()) {
          this.teammatesWithoutNumber.add(pi);
        }
        else {
          this.opponentsWithoutNumber.add(pi);
        }
      }
      else {
        this.playersWithAll.add(pi);
      }
    }

    // do the matching for each type
    this.updatePlayersAfterVI(this.playersWithAll, UPDATE_MODE.SEEN_FULL);
    this.updatePlayersAfterVI(this.teammatesWithoutNumber,
        UPDATE_MODE.SEEN_TEAM_OWN);
    this.updatePlayersAfterVI(this.opponentsWithoutNumber,
        UPDATE_MODE.SEEN_TEAM_OTHER);
    this.updatePlayersAfterVI(this.playersWithoutTeam, UPDATE_MODE.SEEN_NOTEAM);

    // set start position for all players before kick_off
    // TODO improve as it updates twice (rw)
    if (this.world.getPlayMode() == PLAY_MODE.BEFORE_KICK_OFF || this.world
        .getPlayMode() == PLAY_MODE.KICK_OFF_OTHER || this.world.getPlayMode() == PLAY_MODE.KICK_OFF_OWN || this.world
        .getPlayMode() == PLAY_MODE.GOAL_OTHER || this.world.getPlayMode() == PLAY_MODE.GOAL_OWN) {
      this.setTeammatesBeforeKickOff();
    }

    // override expected players if seen, set players that should have been seen
    // outside the viewcone
    for (int playerIndex = 0; playerIndex < this.expectedPlayers.length; playerIndex++) {
      final Player p = this.expectedPlayers[playerIndex];

      // override expected players
      if (this.allPlayers[playerIndex].getBodyCycle() == this.world
          .getBodyCycle()) {
        p.copy(this.allPlayers[playerIndex]);
        continue;
      }

      // players were not seen but expected -> set to !reliable
      if (this.world.shouldBeSeen(p) && !p.isMe()) {
        // try to predict a dashing player and see if this player would
        // be outside the view cone
        if (this.world.getAge(this.allPlayers[playerIndex]) == 1) {
          final Player p_1 = this.allPlayers[playerIndex]
              .predictPlayerAfterAction(this.dash100);
          if (!this.world.shouldBeSeen(p_1)) {
            p.copy(p_1);
            p.setSpeedReliable(false);
            continue;
          }
        }
        // friends are always set unreliable (so hearing them is
        // triggered)
        else if (p.isFriend() || this.world.getSelfRef().getBallPossessions()
            .length() == 0 || p.getDistance(this.allPlayers[playerIndex]) > 25) {

          this.allPlayers[playerIndex].setReliable(false);
          this.allPlayers[playerIndex].setSpeedReliable(false);
          this.allPlayers[playerIndex].getSpeed().reset();

          p.copy(this.allPlayers[playerIndex]);
        }
        else {
          // move the player to our view borders
          this.movePlayerOutsideViewCone(playerIndex);
          this.movePlayerOutsideVisibleDistance(playerIndex);
        }
      }// end should be seen
    }// for all players

    // predict the next players (only done when the ball is near and it's of
    // importance)
    if (this.world.getBallRef().getPosition().y < this.world.getSelfRef()
        .getPosition().y + 30) {
      this.predictNextPlayers();
    }

    // teammates near the ball were obviously not offside before
    this.correctOffsidePlayers();

    this.setDominatedSide();
  }

  /**
   * moves a given player to the edge of the viewcone. does nothing if the
   * player is already outside the viewcone.
   * 
   * @param playerIndex -
   *          the palyers index in the {@link #allPlayers} array.
   */
  private void movePlayerOutsideViewCone(final int playerIndex) {

    final Player p = this.expectedPlayers[playerIndex];

    //if the plyaer was already moved without seeing him againg
    //he is set unreliable
    if(p.getAge(this.world.getBodyCycle(), UPDATE_MODE.META_BASED_ON_INFO) 
    		> p.getAge(this.world.getBodyCycle(), UPDATE_MODE.MOVED)){
        this.allPlayers[playerIndex].setReliable(false);
        this.allPlayers[playerIndex].setSpeedReliable(false);
        this.allPlayers[playerIndex].getSpeed().reset();

        p.copy(this.allPlayers[playerIndex]); 
        return;
    }
    
    final double viewAngleLeftBorder = Vektor.normalize(this.world.getSelfRef()
        .getHeadDir() - this.world.getViewWidth() / 2);
    final double viewAngleRightBorder = Vektor.normalize(this.world.getSelfRef()
        .getHeadDir() + this.world.getViewWidth() / 2);
    final double angleToPlayer = this.world.getSelfRef().getAngleTo(p);

    if (RCMath.isBetween(angleToPlayer, viewAngleLeftBorder,
        viewAngleRightBorder)) {
      final double distanceToPlayer = this.world.getSelfRef().getDistance(p);
      final int age = p.getAge(this.world.getBodyCycle(),
          UPDATE_MODE.META_BASED_ON_INFO);

      // left, dummyVektor contains new pos
      final double resDistance1 = Math.cos(Math
          .toRadians(angleToPlayer - viewAngleLeftBorder)) * distanceToPlayer;
      this.dummyVektor.pointAtPolar(resDistance1, viewAngleLeftBorder);
      this.dummyVektor.addToThis(this.world.getSelfRef().getPosition());
      final double movement1 = p.getDistance(this.dummyVektor);
      if (!this.world.inField(this.dummyVektor) || movement1 > age * 1.2 + resDistance1 / 10) {
        this.dummyVektor.setEmpty();
      }

      // right, dummVektor2 contains new pos
      final double resDistance2 = Math.cos(Math
          .toRadians(viewAngleRightBorder - angleToPlayer)) * distanceToPlayer;
      this.dummyVektor2.pointAtPolar(resDistance2, viewAngleRightBorder);
      this.dummyVektor2.addToThis(this.world.getSelfRef().getPosition());
      final double movement2 = p.getDistance(this.dummyVektor2);
      if (!this.world.inField(this.dummyVektor2) || movement2 > age * 1.2 + resDistance2 / 10) {
        this.dummyVektor2.setEmpty();
      }

      //if a slice next to the current slice was seen in the prev. cycle 
      //we assume that the player is not there
      //the player could of course have crossed the border between both slices
      //in the last cycle but this is very unlikely
      final double ageLeftDir = this.world.getAgeOfDir(viewAngleLeftBorder-20.0);
      final double ageRightDir = this.world.getAgeOfDir(viewAngleRightBorder+20.0);
      
      if(ageLeftDir == 1){
    	  dummyVektor.setEmpty();
    	  if(ageRightDir == 2){
    		  dummyVektor2.setEmpty();
    	  }
      }
      
      if(ageRightDir == 1){
    	  dummyVektor2.setEmpty();
    	  if(ageLeftDir == 2){
    		  dummyVektor.setEmpty();
    	  }
      }
      
      // a movement gets less probable the bigger it gets
      // therefor we filter big movements
      final double distDiff = Math
          .sqrt(distanceToPlayer * distanceToPlayer - resDistance1 * resDistance1) - Math
          .sqrt(distanceToPlayer * distanceToPlayer - resDistance2 * resDistance2);

      if (distDiff > 5) {
        this.dummyVektor.setEmpty();
      }
      else if (distDiff < -5) {
        this.dummyVektor2.setEmpty();
      }

      if (resDistance1 < 3) {
        this.dummyVektor.setEmpty();
      }

      if (resDistance2 < 3) {
        this.dummyVektor2.setEmpty();
      }

      //distance is wrong when vektor is empty but doesn't matter
      final double distanceToGoal1 = this.dummyVektor
          .getDistance(SConf.getInstance().GOAL_POS_OTHER);
      final double distanceToGoal2 = this.dummyVektor2.getDistance(SConf
          .getInstance().GOAL_POS_OTHER);

      // if we are in attack mode we assume that the player moved towards
      // the opponents goal, and we set him to the pos nearer to there
      // otherwise we move the player away from the goal
      if (!this.dummyVektor.isEmpty() && ((this.world.getSelfRef()
          .getBallPossessions().getLast() == TEAM.WE && distanceToGoal1 < distanceToGoal2) || (this.world
          .getSelfRef().getBallPossessions().getLast() != TEAM.WE && distanceToGoal1 > distanceToGoal2) || this.dummyVektor2
          .isEmpty())) {
        p.getSpeed().copy(this.dummyVektor);
        p.getSpeed().subFromThis(p.getPosition());
        p.getSpeed().multThis(1.0 / age);
        if (p.getSpeed().getLength() > p.getPConf().PLAYER_SPEED_MAX) {
          p.getSpeed().setLength(p.getPConf().PLAYER_SPEED_MAX);
        }
        p.setPosition(this.dummyVektor);
        p.getSpeed().reset();
        p.setSpeedReliable(false);
        p.setUpdateMode(UPDATE_MODE.MOVED, this.world.getBodyCycle());
      }
      // moving to left side was not possible or worse -> try moving to
      // the right
      else if (!this.dummyVektor2.isEmpty()) {
        p.getSpeed().copy(this.dummyVektor2);
        p.getSpeed().subFromThis(p.getPosition());
        p.getSpeed().multThis(1.0 / age);
        if (p.getSpeed().getLength() > p.getPConf().PLAYER_SPEED_MAX) {
          p.getSpeed().setLength(p.getPConf().PLAYER_SPEED_MAX);
        }
        p.setPosition(this.dummyVektor2);
        p.getSpeed().reset();
        p.setSpeedReliable(false);
        p.setUpdateMode(UPDATE_MODE.MOVED, this.world.getBodyCycle());
      }
      // if no moving was possible, the player is set unreliable
      else {

        this.allPlayers[playerIndex].setReliable(false);
        this.allPlayers[playerIndex].setSpeedReliable(false);
        this.allPlayers[playerIndex].getSpeed().reset();

        p.copy(this.allPlayers[playerIndex]);
      }
    }// move player to view border

  }

  /**
   * Sets a player outside the visible distance. does nothing if player is not
   * in the visible distance.
   * 
   * @param playerIndex
   */
  private void movePlayerOutsideVisibleDistance(final int playerIndex) {

    final Player p = this.expectedPlayers[playerIndex];

    final double distance = this.world.getSelfRef().getDistance(p);
    if (distance < 3) {
      final double angle = this.world.getSelfRef().getAngleTo(p);
      this.dummyVektor.pointAtPolar(4, angle);
      this.dummyVektor.addToThis(this.world.getSelfRef().getPosition());
      p.setPosition(this.dummyVektor);
      p.getSpeed().reset();
      p.setSpeedReliable(false);
    }
  }

  /**
   * Match seen players to knwon players and update them.
   * 
   * @param infos -
   *          list of all infos belonging to the update mode
   * @param mode -
   *          the update mode (a player can be seen with number and team, with
   *          only the team or without any further information)
   */
  private void updatePlayersAfterVI(final LinkedList<PlayerInfo> infos,
      final UPDATE_MODE mode) {

    // no infos, no update
    if (infos.size() == 0) {
      return;
    }

    // no teammate update if we know where our teammates are
    if (mode == UPDATE_MODE.SEEN_TEAM_OWN && (this.world.getPlayMode() == PLAY_MODE.KICK_OFF_OTHER || this.world
        .getPlayMode() == PLAY_MODE.KICK_OFF_OWN || this.world.getPlayMode() == PLAY_MODE.BEFORE_KICK_OFF)) {
      return;
    }

    // match the new infos against our players knowledge
    final LinkedList<Player> matches = this.matchPlayers(infos, mode);

    // no matches -> return
    // (this case might occure when a player was previously matched
    // to a wrong position. It shouldn't happen to often as we use a
    // high tolerance.)
    if (matches.size() == 0) {
      if (this.DEBUG && this.world.getNumber() == this.DEBUG_PLAYER) {
        System.out
            .println(this.world.getCycleAndPlayerString() + " PlayersModel: No Matches for " + infos
                .size() + " PlayerInfos. Mode" + mode.toString());
        this.printMatchMatrix(infos);
      }
      return;
    }

    // this might occure as we have a limit for the calculation depth
    // as the problem is of o(n!)
    if (infos.size() != matches.size()) {
      if (this.DEBUG && this.world.getNumber() == this.DEBUG_PLAYER) {
        System.out
            .println(this.world.getCycleAndPlayerString() + " PlayersModel: No equal Number of Matches found. Infos:" + infos
                .size() + " Matches:" + matches.size() + "Mode" + mode
                .toString());
        this.printMatchMatrix(infos);
      }
      return;
    }

    if (this.DEBUG && this.world.getNumber() == this.DEBUG_PLAYER && mode != UPDATE_MODE.SEEN_FULL) {
      System.out.println(" UM:" + mode.toString());
      this.printMatchMatrix(infos);
    }

    // update matched players
    for (int i = 0; i < infos.size(); i++) {

      final Player nearestPlayer = matches.get(i);

      // if we heared the player in this cycle then
      // we update only if the number is given (no mismatch possible)
      if (nearestPlayer.getAge(this.world.getBodyCycle(), UPDATE_MODE.HEARED) == 0 && mode != UPDATE_MODE.SEEN_FULL) {
        if (this.DEBUG && this.world.getNumber() == this.DEBUG_PLAYER) {
          System.out
              .println(" Skipped " + nearestPlayer.getNumber() + " " + nearestPlayer
                  .getAge(this.world.getBodyCycle(), UPDATE_MODE.HEARED));
        }
        continue;
      }

      // if the opponents goalie is seen for the 1st time its number is
      // saved
      if (mode == UPDATE_MODE.SEEN_FULL && infos.get(i).isGoalie() && !infos
          .get(i).isFriend()) {
        if (this.numberGoalieOpponent < 0) {
          this.numberGoalieOpponent = infos.get(i).getNumber();
          this.allPlayers[infos.get(i).getNumber() + 11 - 1].setGoalie(true);
          this.expectedPlayers[infos.get(i).getNumber() + 11 - 1]
              .setGoalie(true);
          this.nextPlayers[infos.get(i).getNumber() + 11 - 1].setGoalie(true);
        }
      }

      // the goalie is only updated if the match is also seen as goalie
      // (preventing strange things....)
      if (infos.get(i).isGoalie() && !nearestPlayer.isGoalie()) {
        continue;
      }

      if (nearestPlayer.isGoalie() && !infos.get(i).isGoalie() && this.world
          .getDistance(nearestPlayer) < 30 && !this.world
          .inOpponentsPenalty(nearestPlayer)) {
        continue;
      }

      final PlayerInfo info = infos.get(i);

      this.newPlayerPosAbs.copy(info.getAbsPlayerPosition());

      // absolute body angle
      // if body or neck angle are not given the old values are used!!
      double bodyAngle = info.getBodyDir();

      if (bodyAngle != DAInamiteConstants.DOUBLE_UNKNOWN) {
        // bodyAngle += me.getBodyDir();
        // the players global body angle is been seen relative to my
        // head
        // -> add my head angle
        bodyAngle += this.world.getSelfRef().getHeadDir();
        bodyAngle = Vektor.normalize(bodyAngle);
        nearestPlayer.setBodyDir(bodyAngle);
      } // end else

      // neck angle (relative to body angle)
      double neckAngle = info.getHeadDir();
      if (neckAngle != DAInamiteConstants.DOUBLE_UNKNOWN) {
        // neckAngle += me.getNeckAngle();
        // the players global neck angle is been seen relative to my
        // head
        // -> add my head angle
        neckAngle += this.world.getSelfRef().getHeadDir();
        // now subtract his own body angle
        neckAngle -= bodyAngle;
        neckAngle = Vektor.normalize(neckAngle);
        if (neckAngle > this.sConf.MAXNECKANG) {
          neckAngle = this.sConf.MAXNECKANG;
        }
        else if (neckAngle < this.sConf.MINNECKANG) {
          neckAngle = this.sConf.MINNECKANG;
        }
        nearestPlayer.setNeckAngle(neckAngle);
      } // end else

      // set only if all values are known, and the distance is small
      // else the noise is too big!
      double pointDir = info.getPointDir();
      if (pointDir != DAInamiteConstants.DOUBLE_UNKNOWN && bodyAngle != DAInamiteConstants.DOUBLE_UNKNOWN && neckAngle != DAInamiteConstants.DOUBLE_UNKNOWN && info
          .getDistance() < DAInamiteConstants.MAX_POINT_AT_NOTICE_DIST) {
        // double angleTo =
        // world.getSelfRef().getAngleTo(nearestPlayer);
        pointDir = Vektor.normalize(pointDir + this.world.getSelfRef()
            .getHeadDir());

        double maxAngle = DAInamiteConstants.STAMINA_MAX_POINT_ANGLE;
        if (pointDir > -maxAngle) {
          nearestPlayer.setArmDirection(pointDir);
          // update stamina as well:
          if (nearestPlayer.isFriend()) {
            double stamina = ((pointDir + maxAngle) / (2 * maxAngle)) * DAInamiteConstants.MAX_STAMINA_POINTED;
            if (stamina < 400) {
              stamina = 400;
            }
            nearestPlayer.setStamina(stamina);
          }
        }
      }

      // calculate speed
      this.world.calculateVelocity(info, this.newPlayerSpeedAbs);

      if (!this.newPlayerSpeedAbs.isEmpty()) {
        nearestPlayer.setSpeed(this.newPlayerSpeedAbs);
        nearestPlayer.setSpeedReliable(true);
      }
      else {
        this.estimatePlayersSpeed(nearestPlayer, this.newPlayerPosAbs,
            this.world.getBodyCycle());
        this.estimatePlayersStamina(nearestPlayer, nearestPlayer.getPosition(),
            this.newPlayerPosAbs);
      }

      // set body angle in speed dir
      if (bodyAngle == DAInamiteConstants.DOUBLE_UNKNOWN && nearestPlayer
          .getSpeed().getLength() != 0) {
        nearestPlayer.setBodyDir(nearestPlayer.getSpeed().getAngle());
      }

      // normalize speed
      if (nearestPlayer.getSpeed().getLength() > nearestPlayer.getPConf().PLAYER_SPEED_MAX * nearestPlayer
          .getPConf().PLAYER_DECAY) {
        nearestPlayer.getSpeed().setLength(
            nearestPlayer.getPConf().PLAYER_SPEED_MAX * nearestPlayer
                .getPConf().PLAYER_DECAY);
      }

      // update the player (nearest player contains a reference!!)
      nearestPlayer.setPosition(this.newPlayerPosAbs);
      if (mode == UPDATE_MODE.SEEN_FULL) {
        nearestPlayer.setLastSeenPosition(this.newPlayerPosAbs);
        nearestPlayer.setLastSeenBodyCycle(this.world.getBodyCycle());
      }
      nearestPlayer.setReliable(true);
      nearestPlayer.setCycle(this.world.getCycle());
      nearestPlayer.setBodyCycle(this.world.getBodyCycle());
      // remember what kind of knowledge we used for the update
      nearestPlayer.setUpdateMode(mode, this.world.getBodyCycle());
      if (info.isTackle()) {
        if (nearestPlayer.isFriend()) {
          nearestPlayer.setTackledeadCountdown(Math.max(0,
              SConf.getInstance().TACKLE_CYCLES - 1 - nearestPlayer.getAge(
                  this.world.getBodyCycle(), UPDATE_MODE.SEEN_TEAM_OWN)));
        }
        else {
          nearestPlayer.setTackledeadCountdown(Math.max(0,
              SConf.getInstance().TACKLE_CYCLES - 1 - nearestPlayer.getAge(
                  this.world.getBodyCycle(), UPDATE_MODE.SEEN_TEAM_OTHER)));
        }
      }

      /**
       * if the player told us he was dribbling then we assume that he must be
       * near the ball. this is important as due to noise the ball might be seen
       * outside this player's kickable area leading to strange predictions for
       * the ball interception
       */
      if (this.world.getSelfRef().getLastHearedPlayer() == nearestPlayer
          .getNumber() && nearestPlayer.isFriend()) {
        if (this.world.getSelfRef().getLastHearBodyCycle() == this.world
            .getBodyCycle() && this.world.getSelfRef()
            .getLastHearedMessageType() == MESSAGES.DRIBBLE_BALL && !nearestPlayer
            .canKick(this.world.getBallRef())) {
          final Vektor playerToBall = this.world.getBallRef().getPosition().sub(
              nearestPlayer.getPosition());
          playerToBall.setLength(0.6);
          nearestPlayer.setPosition(this.world.getBallRef().getPosition());
          nearestPlayer.getPosition().addToThis(playerToBall);
        }
      }
    }

  }

  /**
   * estimates the players speed based on the last 2 seen or heared positions.
   * 
   * @param player -
   *          the player (the speed is set to this reference!!!)
   * @param vektor -
   *          the new seen position (the old position is in the player)
   */
  private void estimatePlayersSpeed(final Player player,
      final Vektor newPos,
      final int infoCycle) {

    if (this.world.getPlayMode() != PLAY_MODE.PLAY_ON) {
      player.getSpeed().reset();
      player.setSpeedReliable(false);
      return;
    }

    // if no speed is given it gets estimated from the players global
    // movement
    final int age = player.getAge(this.world.getBodyCycle(),
        UPDATE_MODE.META_BASED_ON_INFO) + infoCycle - this.world.getBodyCycle();

    if (age > 0) {
      player.getSpeed().x = (double) (newPos.x - player.getPosition().x) / age;
      player.getSpeed().y = (double) (newPos.y - player.getPosition().y) / age;

//      if (age == 1) {
//        player.getSpeed().subFromThis(
//            this.world.getSelfPositionEstimationError());
//      }

      // player was seen to fast -> decrease speed
      if (player.getSpeed().getLength() > player.getPConf().PLAYER_SPEED_MAX) {
        player.getSpeed().setLength(player.getPConf().PLAYER_SPEED_MAX);
      }

      player.getSpeed().multThis(player.getPConf().PLAYER_DECAY);
      player.setSpeedReliable(false);
    }

  }

  /**
   * estimates the players stamina base on the last 2 seen or heared positions.
   * 
   * @param player -
   *          the player (the stamina is set to this the reference!!!)
   * @param vektor -
   *          the new seen position (the old position is in the player)
   */
  private void estimatePlayersStamina(final Player player,
      final Vektor oldPos,
      final Vektor newPos) {

	// do not update in these cases:
	// 1. Friends communicate stamina
	if (player.isFriend())
		return;
	// 2. Save Stamina in other play-modes
	if (world.getPlayMode() != PLAY_MODE.PLAY_ON)
		return;
	// 3. Movement seen, where no movement is (noise)
	if (world.getDistance(player) > 25)
		return;
	  
    // if no speed is given it gets estimated from the players global
    // movement
    final int age = player.getAge(this.world.getBodyCycle(),
        UPDATE_MODE.META_BASED_ON_INFO);

    if (age > 1) {

      this.dummyVektor.copy(newPos);
      this.dummyVektor.subFromThis(oldPos);

      // estimating the players stamina
      final double movement = this.dummyVektor.getLength();

      double stamina = age * (player.getPConf().STAMINA_INC_MAX + 5);
      stamina -= movement / player.getPConf().PLAYER_SPEED_MAX * 105;
      stamina += player.getStamina();

      if (stamina > SConf.getInstance().STAMINA_MAX) {
        stamina = SConf.getInstance().STAMINA_MAX;
      }

      if (stamina < DAInamiteConstants.STAMINA_THR_LOW) {
        stamina = DAInamiteConstants.STAMINA_THR_LOW;
      }

      player.setStamina(stamina);

    }

  }

  /**
   * Matches the given infos against our player knowledge.
   * 
   * @param infos -
   *          list of all infos belonging to the update mode
   * @param mode -
   *          the update mode (a player can be seen with number and team, with
   *          only the team or without any further information)
   * 
   * @return a list with the matched player in order of the given infos (same as
   *         this.matchedPlayersBuffer)
   */
  private LinkedList<Player> matchPlayers(final LinkedList<PlayerInfo> infos,
      final UPDATE_MODE mode) {

    // reset
    this.matchedPlayersBuffer.clear();

    // no infos, no matching
    if (infos == null || infos.size() == 0) {
      return this.matchedPlayersBuffer;
    }

    // matching depends on update type
    switch (mode) {
      // the easiest case with full information given
      case SEEN_FULL:
        for (final PlayerInfo info : infos) {
          if (info.isFriend()) {
            this.matchedPlayersBuffer
                .add(this.allPlayers[info.getNumber() - 1]);
          }
          else {
            this.matchedPlayersBuffer
                .add(this.allPlayers[info.getNumber() - 1 + 11]);
          }
        }
        break;
      // everything else gets more complicated
      default:
        // load the distance matrix (Player X Infos)
        this.loadDistances(infos, mode);

        // filter distances that couldn't be reached
        this.filterDistances(infos);

        // square remaining distances for better matching
        this.squareDistances();

        // int values

        // sums[0] = currentValue
        // sums[1] = bestValue
        final double[] sums = new double[] { 0.0, 1000000 };

        // an array containing the playerIndexes of the way taken by the
        // algorithm
        final int[] wayTaken = new int[this.matchDistancesRowCount];
        Arrays.fill(wayTaken, -1);

        // an array containing the best way taken by the algorithm
        final int[] bestWay = new int[this.matchDistancesRowCount];
        Arrays.fill(bestWay, -1);

        // an array which tells which players were already matched
        final boolean[] columnUsed = new boolean[this.matchDistancesColumnCount];
        Arrays.fill(columnUsed, false);

        // a security counter as we could get n! ways (which should not happen
        // to often as we filter)
        final int[] counter = new int[] { 0, this.matchCounterLimit };

        // the the recursive algorithm
        this.matchLoop(0, sums, wayTaken, bestWay, columnUsed, counter);

        if (this.DEBUG && this.world.getNumber() == this.DEBUG_PLAYER) {
          System.out.println(" BestWay:" + Arrays.toString(bestWay) + Arrays
              .toString(sums) + Arrays.toString(wayTaken) + Arrays
              .toString(counter));
        }

        // no matches found
        if (bestWay[0] < 0) {
          this.matchedPlayersBuffer.clear();
        }
        // matches found -> added the players to the return list
        else {
          Player p;
          for (final int i : bestWay) {
            p = this.allPlayers[(int) this.matchDistances[0][i]];
            this.matchedPlayersBuffer.add(p);
          }
        }
    }

    return this.matchedPlayersBuffer;
  }

  /**
   * updates the matrix containing all distances from Players to info positions.
   * Players which have already been updated in this cycle are filtered. the
   * method also sets the limits (size) of the matrix. The distance is taken to
   * the last seen and not to the predicted position of a player!!
   * 
   * @param infos -
   *          list of all infos belonging to the update mode
   * @param mode -
   *          the update mode (a player can be seen with number and team, with
   *          only the team or without any further information)
   */
  private void loadDistances(final LinkedList<PlayerInfo> infos,
      final UPDATE_MODE mode) {

    int firstIndex = 0;
    int lastIndex = 21;

    if (mode == UPDATE_MODE.SEEN_TEAM_OTHER) {
      firstIndex = 11;
    }
    else if (mode == UPDATE_MODE.SEEN_TEAM_OWN) {
      lastIndex = 10;
    }

    int columnIndex = 0;

    Player p;
    int age = 0;

    // fill the "Player X Info" matrix starting with the columns
    for (int playerIndex = firstIndex; playerIndex <= lastIndex; playerIndex++) {
      p = this.allPlayers[playerIndex];

      // as we have different update modes we also have different age
      // types
      // player are only filtered with age 1, so every other value is used
      // for including them
      switch (mode) {
        // always update, when fully seen
        case SEEN_FULL:
          age = 1;
          break;
        // use only players that were not fully seen
        case SEEN_TEAM_OTHER:
        case SEEN_TEAM_OWN:
          age = p.getAge(this.world.getBodyCycle(), UPDATE_MODE.SEEN_FULL);
          break;
        // use only players that were not fully seen or seen with team
        case SEEN_NOTEAM:
          age = p.getAge(this.world.getBodyCycle(), UPDATE_MODE.SEEN_FULL);
          if (p.isFriend()) {
            age = Math.min(age, p.getAge(this.world.getBodyCycle(),
                UPDATE_MODE.SEEN_TEAM_OWN));
          }
          else {
            age = Math.min(age, p.getAge(this.world.getBodyCycle(),
                UPDATE_MODE.SEEN_TEAM_OTHER));
          }
      }

      // skip filtered players
      if (age == 0 || p.isMe()) {
        continue;
      }
      // calculate the distance player/info-position otherwise and
      // fill the matrix
      else {
        this.matchDistances[0][columnIndex] = playerIndex;
        for (int rowIndex = 0; rowIndex < infos.size(); rowIndex++) {
          this.matchDistances[rowIndex + 1][columnIndex] = p.getDistance(infos
              .get(rowIndex).getAbsPlayerPosition());
        }
        columnIndex++;
      }
    }

    // remember the size of the matrix for further calculations
    this.matchDistancesColumnCount = columnIndex;
    this.matchDistancesRowCount = infos.size();
  }

  /**
   * This method returns a <code>LinkedList</code> of all known teammates that
   * are at the given <code>angle</code> plus/minus <code>deviation</code>
   * and within the given <code>distance</code>. Note that if you give a
   * <code>deviation</code> of 180, you'll get all teammates within a circle
   * around the player with the radius <code>
   * maxDistance</code>.
   * 
   * @param position -
   *          location
   * @param angle -
   *          the dir of the cone
   * @param deviation -
   *          half cone width in degree
   * @param maxDistance -
   *          cone length
   * @return LinkedList of <code>Player</code>s inside the cone
   */
  public final LinkedList<Player> getTeammates(final Vektor position,
      final double angle,
      final double deviation,
      final double maxDistance) {

    final LinkedList<Player> friends = new LinkedList<Player>();
    for (final Player p : this.teammates) {
      // TODO define reliability (rw)
      if (p.isReliable()) {
        final double angleToOpp = position.getAngleTo(p.getPosition());
        if (this.world.getDistance(p) <= maxDistance && Math.abs(Vektor
            .normalize(angleToOpp - angle)) <= deviation) {
          friends.add(p);
        }
      }
    }
    return friends;
  }

  /**
   * filters distances which were unreachable. used for the players X playerInfo
   * matching. Impossible combinations are set to {@link #emptyValue} in the
   * matrix.
   * 
   * @param infos -
   *          the visual player infos
   */
  private void filterDistances(final LinkedList<PlayerInfo> infos) {

    // the age of the last update
    int age;

    // the max distance reachable in the given age by the player
    double maxDist;

    // for each player...
    for (int columnIndex = 0; columnIndex < this.matchDistancesColumnCount; columnIndex++) {
      final Player p = this.allPlayers[(int) this.matchDistances[0][columnIndex]];
      // the simple age
      age = p.getAge(this.world.getBodyCycle(), UPDATE_MODE.META_BASED_ON_INFO);
      for (int rowIndex = 0; rowIndex < this.matchDistancesRowCount; rowIndex++) {
        // use (high) tolerance as all seen positions contain noise
        // (which depends on the distance)
        maxDist = Math.min(
            (2.0 + age + infos.get(rowIndex).getDistance() / 10.0) * p
                .getPConf().PLAYER_SPEED_MAX, this.maxMatchDistance);
        if (maxDist < this.matchDistances[rowIndex + 1][columnIndex]) {
          this.matchDistances[rowIndex + 1][columnIndex] = this.emptyValue;
        }
      }
    }
  }

  /**
   * simple method which squares all distances of the match matrix for a smaler
   * error.
   */
  private void squareDistances() {

    for (int columnIndex = 0; columnIndex < this.matchDistancesColumnCount; columnIndex++) {
      for (int rowIndex = 0; rowIndex < this.matchDistancesRowCount; rowIndex++) {
        if (this.matchDistances[rowIndex + 1][columnIndex] > 0) {
          this.matchDistances[rowIndex + 1][columnIndex] *= this.matchDistances[rowIndex + 1][columnIndex];
        }
      }
    }

  }

  /**
   * recursive method that finds the best matches by trying to minimize the
   * error sum. (this method doesn't return any values instead all the results
   * are set to the given params)
   * 
   * @param rowIndex -
   *          the row (info) index
   * @param sums -
   *          sums[0] = current sum, sum[1] = bestSum of all distances
   * @param wayTaken -
   *          the way in the matrix that has been taken. this array contains the
   *          matching playerNumbers in the order of the infos.
   * @param bestWay -
   *          the way that belongs to the bestSum found
   * @param columnUsed -
   *          used columns (players) are set to false and not considered anymore
   * @param counter -
   *          a security counter, counter[0] = currentCounter, counter[1] =
   *          {@link #matchCounterLimit}
   * 
   */
  public void matchLoop(final int rowIndex,
      final double[] sums,
      final int[] wayTaken,
      final int[] bestWay,
      final boolean[] columnUsed,
      final int[] counter) {

    final double currentSum = sums[0];
    final double bestSum = sums[1];

    // got a result -> save it if best
    if (rowIndex == this.matchDistancesRowCount) {
      if (currentSum < bestSum) {
        System.arraycopy(wayTaken, 0, bestWay, 0, bestWay.length);
        sums[1] = currentSum;
      }
      return;
    }

    double newSum;

    // for all players (columns)
    for (int columnIndex = 0; columnIndex < this.matchDistancesColumnCount; columnIndex++) {

      // filtered or used -> skip player
      if (this.matchDistances[rowIndex + 1][columnIndex] < 0 || columnUsed[columnIndex]) {
        continue;
      }

      // get the new sum
      newSum = currentSum + this.matchDistances[rowIndex + 1][columnIndex];
      // if the new Sum is too bad skip the way
      if (newSum > bestSum) {
        continue;
      }

      columnUsed[columnIndex] = true;
      wayTaken[rowIndex] = columnIndex;
      sums[0] = newSum;
      counter[0] = counter[0] + 1;

      // just in case....
      if (counter[0] > counter[1]) {
        return;
      }

      // go deeper and repeat recursion
      this
          .matchLoop(rowIndex + 1, sums, wayTaken, bestWay, columnUsed, counter);
      columnUsed[columnIndex] = false;
    }
  }

  /*
   * (non-Javadoc)
   * 
   * @see robocup.component.worldmodel.IModel#update(robocup.component.infotypes.RefereeInfo)
   */
  public void update(final RefereeInfo info) {

  }

  /**
   * updatePlayersAural updates all players Vectors from the AuralInfo
   * <code>aInfo</code>
   * 
   * @param info -
   *          Info for the update
   */
  public void update(final PlayModeInfo info) {

    // set start position for all players before kick_off
    // TODO improve as it updates twice (rw)
    if (info.getPlayModeType() == PLAY_MODE.BEFORE_KICK_OFF || info
        .getPlayModeType() == PLAY_MODE.KICK_OFF_OTHER || info
        .getPlayModeType() == PLAY_MODE.KICK_OFF_OWN || info.getPlayModeType() == PLAY_MODE.GOAL_OTHER || info
        .getPlayModeType() == PLAY_MODE.GOAL_OWN) {
      this.setTeammatesBeforeKickOff();
    }
  }

  /**
   * predicts the current state, the next action and the resulting player for
   * each player except ourself. If no next action is predicted the next player
   * will be the same as the current player
   * 
   * the predicted players are set to the nextPlayers array
   * 
   */
  private void predictNextPlayers() {

    if (this.world.getBallRef() == null) {
      return;
    }

    for (int i = 0; i < this.expectedPlayers.length; i++) {

      this.nextPlayers[i].copy(this.expectedPlayers[i]);

      if (this.expectedPlayers[i].isMe() || this.expectedPlayers[i]
          .getMyStateEval() == null || !this.expectedPlayers[i].isReliable()) {
        continue;
      }

      this.nextPlayers[i].calculateBestState(this.world.getBallRef());
      final AbstractState state = this.nextPlayers[i].getMyStateEval().getBestState();

      if (state != null && state.getState() != STATES.SEARCH_BALL) {
        state.calculateMainAction();
        this.nextPlayers[i].predictThisAfterState(state);
      }
    }
  }

  /**
   * sets the positions off teammates before kick off based on our formation.
   * 
   */
  public void setTeammatesBeforeKickOff() {

    if (SConf.getInstance().COACH) {
      return;
    }

    // we kick
    final boolean selfKickOff = (this.world.isTeamEast() && this.world.getPlayMode() == PLAY_MODE.BEFORE_KICK_OFF) || this.world
        .getPlayMode() == PLAY_MODE.KICK_OFF_OWN;

    for (final Player p : this.teammates) {
      if (!p.isMe()) {
    	p.setPosition(DefaultPositions.getStartPosition(
    			p.getDefaultRole(), 
    			selfKickOff));
        p.setLastSeenPosition(p.getPosition());
        p.setBodyCycle(this.world.getBodyCycle());
        p.setCycle(this.world.getCycle());
        p.setUpdateMode(UPDATE_MODE.INIT, this.world.getBodyCycle());
        if (this.world.getPlayMode() == PLAY_MODE.BEFORE_KICK_OFF) {
          p.setRecovery(this.sConf.RECOVER_INIT);
        }
      }
    }
  }

  /**
   * This method updates the player-model due to informations communicated by
   * other players!
   * 
   * @param psi -
   *          a player-say information!
   */
  public void update(final PlayerSayInfo psi) {

    final MessageFactory mf = this.world.getMessageFactory();
    final LinkedList<MESSAGES> types = mf.getMessageTypes();

    Vektor ballPos = null;
    int playerIndex = -1;
    int infoCycleRel;

    // for all received messages
    for (int i = 0; i < types.size(); i++) {
      final MESSAGES m = types.get(i);

      // each message type needs a special update logic
      switch (m) {
        // a complete player is sent (position+speed)
        case COMPLETE_PLAYER:
          mf.decodeMessageNumber(i);

          playerIndex = mf.getNumber() - 1;
          infoCycleRel = mf.getAge();

          if (this.allPlayers[playerIndex].isMe() || this.allPlayers[playerIndex]
              .getAge(this.world.getBodyCycle(), UPDATE_MODE.SEEN_FULL) < 2) {
            continue;
          }

          this.estimatePlayersStamina(this.allPlayers[playerIndex],
              this.allPlayers[playerIndex].getPosition(), mf.getPosition());

          this.allPlayers[playerIndex].getPosition().copy(mf.getPosition());
          this.allPlayers[playerIndex].getSpeed().copy(mf.getSpeed());
          this.allPlayers[playerIndex].setBodyDir(mf.getBodyAngle());
          this.allPlayers[playerIndex].setReliable(true);
          this.allPlayers[playerIndex]
              .setBodyCycle(this.world.getBodyCycle() - 1 + infoCycleRel);
          this.allPlayers[playerIndex]
              .setCycle(this.world.getCycle() - 1 + infoCycleRel);
          this.allPlayers[mf.getNumber() - 1].setUpdateMode(UPDATE_MODE.HEARED,
              this.world.getBodyCycle() - 1 + infoCycleRel);
          this.expectedPlayers[playerIndex].copy(this.allPlayers[playerIndex]);
          break;

        // someone told us has was dribbling sending us his position (but no
        // speed)
        case DRIBBLE_PPOS:
          mf.decodeMessageNumber(i);

          final Vektor[] positions = mf.getPositions();
          final int[] numbers = mf.getNumbers();

          for (int idx = 0; idx < mf.getPositions().length; idx++) {

            final Player p = this.allPlayers[numbers[idx] - 1];
            infoCycleRel = mf.getAge();

            if (p.isMe()) {
              continue;
            }

            if (p.getAge(this.world.getBodyCycle(),
                UPDATE_MODE.META_BASED_ON_INFO) == 0) {
              continue;
            }

            if ((p.getNumber() != psi.getNumber() || !p.isFriend())) {
              if (p.getAge(this.world.getBodyCycle(),
                  UPDATE_MODE.META_BASED_ON_INFO) == 1) {
                final Player speaker = this.world.getExpectedPlayers()[psi
                    .getNumber() - 1];
                if (speaker.getDistance(p) > this.world.getSelfRef()
                    .getDistance(p) - 4) {
                  continue;
                }
              }
            }

            this.estimatePlayersStamina(p, p.getPosition(), positions[idx]);
            this.estimatePlayersSpeed(p, positions[idx],
                this.world.getCycle() - 1 + infoCycleRel);

            p.getPosition().copy(positions[idx]);
            if (p.getSpeed().getLength() > 0.1) {
              p.setBodyDir(p.getSpeed().getAngle());
            }

            p.setReliable(true);
            p.setBodyCycle(this.world.getBodyCycle() - 1 + infoCycleRel);
            p.setCycle(this.world.getCycle() - 1 + infoCycleRel);
            p.setUpdateMode(UPDATE_MODE.HEARED,
                this.world.getBodyCycle() - 1 + infoCycleRel);
            this.expectedPlayers[numbers[idx] - 1].copy(p);

            if (p.isFriend()) {
              this.setPlayersOffside(numbers[idx] - 1);
            }
            else {
              this.removePlayersOffside();
            }

            if (idx == 0) {
              this.turnOffStandardPlayer(numbers[idx] - 1);
            }

          }
          break;

        // someone sent us a kicked ball. the last ball position mus be near the
        // kicking players position
        case KICKED_BALL:
          mf.decodeMessageNumber(i);
          // sender was kicking
          playerIndex = psi.getNumber() - 1;
          final Player kickingPlayer = this.allPlayers[playerIndex];

          if (kickingPlayer.getAge(this.world.getBodyCycle(),
              UPDATE_MODE.SEEN_FULL) < 2) {
            continue;
          }

          ballPos = mf.getPosition();
          // the ball pos when kicking (~= kickers pos)
          ballPos.subFromThis(mf.getSpeed().mult(
              1.0 / SConf.getInstance().BALL_DECAY));
          if (this.allPlayers[playerIndex].getDistance(ballPos) > 0.9) {
            final Vektor playerToBallAbs = new Vektor(0.5, 0);
            this.allPlayers[playerIndex].setPosition(ballPos
                .sub(playerToBallAbs));
          }
          // kicking kind of stops the player
          this.allPlayers[playerIndex].getSpeed().reset();
          this.allPlayers[playerIndex].setBodyDir(mf.getBodyAngle());
          this.allPlayers[playerIndex].setReliable(true);
          this.allPlayers[playerIndex]
              .setBodyCycle(this.world.getBodyCycle() - 1);
          this.allPlayers[playerIndex].setCycle(this.world.getCycle() - 1);
          this.allPlayers[mf.getNumber() - 1].setUpdateMode(UPDATE_MODE.HEARED,
              this.world.getBodyCycle() - 1);
          this.expectedPlayers[playerIndex].copy(this.allPlayers[playerIndex]);
          this.setPlayersOffside(playerIndex);
          this.turnOffStandardPlayer(playerIndex);
          break;

        // a simple player position (1 message can encode max. 2 positions)
        case PLAYER_POS:
          mf.decodeMessageNumber(i);
          playerIndex = mf.getNumber() - 1;

          if (this.allPlayers[playerIndex].isMe()) {
            continue;
          }

          Player p = this.allPlayers[playerIndex];

          if (p.isMe()) {
            continue;
          }

          if ((p.getNumber() != psi.getNumber() || !p.isFriend())) {
            if (p.getAge(this.world.getBodyCycle(),
                UPDATE_MODE.META_BASED_ON_INFO) == 0) {
              continue;
            }

            if (p.getAge(this.world.getBodyCycle(),
                UPDATE_MODE.META_BASED_ON_INFO) == 1) {
              final Player speaker = this.world.getExpectedPlayers()[psi.getNumber() - 1];
              if (speaker.getDistance(p) > this.world.getSelfRef().getDistance(
                  p) - 4) {
                continue;
              }
            }
          }

          infoCycleRel = 0;
          int playerAge = this.world.getAge(this.allPlayers[playerIndex]);

          // estimate players speed by the players movement

          // should never happen
          if (playerAge == 0) {
            this.allPlayers[playerIndex].getSpeed().reset();
          }
          else {
            // Player intercepts ball so we "know" the speed
            if (this.world.getExpectedPlayers()[playerIndex].getLastState() == STATES.INTERCEPT_BALL) {
              // System.out.println(world.getCycleAndPlayerString() +
              // " PM: Left Speed as mate intercepts ball " +
              // playerIndex);
              this.dummyVektor
                  .copy(this.world.getExpectedPlayers()[playerIndex].getSpeed());
              this.allPlayers[playerIndex].getSpeed().copy(this.dummyVektor);
            }
            else {
              this.estimatePlayersStamina(p, p.getPosition(), mf.getPosition());
              this.estimatePlayersSpeed(p, mf.getPosition(), this.world
                  .getCycle() - 1 + infoCycleRel);
            }
          }

          this.allPlayers[playerIndex].getPosition().copy(mf.getPosition());
          this.allPlayers[playerIndex].setReliable(true);
          this.allPlayers[playerIndex]
              .setBodyCycle(this.world.getBodyCycle() - 1 + infoCycleRel);
          this.allPlayers[playerIndex]
              .setCycle(this.world.getCycle() - 1 + infoCycleRel);
          this.allPlayers[playerIndex].setBodyDir(this.dummyVektor.getAngle());
          this.allPlayers[playerIndex].setUpdateMode(UPDATE_MODE.HEARED,
              this.world.getBodyCycle() - 1 + infoCycleRel);
          this.expectedPlayers[playerIndex].copy(this.allPlayers[playerIndex]);

          break;

        // a player position and the players (discretized) stamina level
        case PLAYER_POS_STAMINA:
          mf.decodeMessageNumber(i);
          playerIndex = psi.getNumber() - 1;

          if (this.allPlayers[playerIndex].isMe()) {
            continue;
          }

          p = this.allPlayers[playerIndex];

          if ((p.getNumber() != psi.getNumber() || !p.isFriend())) {
            if (p.getAge(this.world.getBodyCycle(),
                UPDATE_MODE.META_BASED_ON_INFO) == 0) {
              continue;
            }

            if (p.getAge(this.world.getBodyCycle(),
                UPDATE_MODE.META_BASED_ON_INFO) == 1) {
              final Player speaker = this.world.getExpectedPlayers()[psi.getNumber() - 1];
              if (speaker.getDistance(p) > this.world.getSelfRef().getDistance(
                  p) - 4) {
                continue;
              }
            }
          }

          infoCycleRel = 0;
          playerAge = this.world.getAge(this.allPlayers[playerIndex]);

          // estimate players speed by the players movement

          // should never happen
          if (playerAge == 0) {
            this.allPlayers[playerIndex].getSpeed().reset();
          }
          else {
            // Player intercepts ball so we "know" the speed
            if (this.world.getExpectedPlayers()[playerIndex].getLastState() == STATES.INTERCEPT_BALL) {
              // System.out.println(world.getCycleAndPlayerString() +
              // " PM: Left Speed as mate intercepts ball " +
              // playerIndex);
              this.dummyVektor
                  .copy(this.world.getExpectedPlayers()[playerIndex].getSpeed());
              this.allPlayers[playerIndex].getSpeed().copy(this.dummyVektor);
            }
            else {
              this.estimatePlayersStamina(p, p.getPosition(), mf.getPosition());
              this.estimatePlayersSpeed(p, mf.getPosition(), this.world
                  .getCycle() - 1 + infoCycleRel);
            }
          }

          this.allPlayers[playerIndex].getPosition().copy(mf.getPosition());
          this.allPlayers[playerIndex].setStamina(mf.getStamina());
          this.allPlayers[playerIndex].setReliable(true);
          this.allPlayers[playerIndex]
              .setBodyCycle(this.world.getBodyCycle() - 1 + infoCycleRel);
          this.allPlayers[playerIndex]
              .setCycle(this.world.getCycle() - 1 + infoCycleRel);
          this.allPlayers[playerIndex].setBodyDir(this.dummyVektor.getAngle());
          this.allPlayers[playerIndex].setUpdateMode(UPDATE_MODE.HEARED,
              this.world.getBodyCycle() - 1 + infoCycleRel);
          this.expectedPlayers[playerIndex].copy(this.allPlayers[playerIndex]);

          break;

        default:
          ;
      }

    }
  }

  /**
   * sets a flag to a player that did a standard so we know that he is not
   * allowed to handle the ball before an other player had it.
   */
  private void turnOffStandardPlayer(final int playerIndex) {

    for (int i = 0; i < 22; i++) {
      this.expectedPlayers[i].setKickedStandard(false);
      this.allPlayers[i].setKickedStandard(false);
      this.nextPlayers[i].setKickedStandard(false);
    }

    if ((PLAY_MODE.META_STANDARD_OWN.contains(this.world.getPreviousPlaymode()) || PLAY_MODE.META_STANDARD_OTHER
        .contains(this.world.getPreviousPlaymode())) && this.world
        .getPlayMode() == PLAY_MODE.PLAY_ON && this.world
        .getLastPlayModeMessageCycle() > this.world.getBodyCycle() - 2) {
      this.expectedPlayers[playerIndex].setKickedStandard(true);
      this.allPlayers[playerIndex].setKickedStandard(true);
      this.nextPlayers[playerIndex].setKickedStandard(true);
    }

  }

  /**
   * sets the offside flag to players when a passe occured
   * 
   * @param playerKicking -
   *          the index of the player that kicked (and therefor can not be
   *          offside)
   * 
   * Not working yet!!!
   */
  private void setPlayersOffside(final int playerKicking) {

    if (true) {
      return;
    }

    final double offsideY = this.world.getOffsideY(true) + 1;

    for (int i = 0; i < 11; i++) {
      if (i != playerKicking && this.expectedPlayers[i].getPosition().y > offsideY) {
        this.expectedPlayers[i].setWasOffsideWhenPassed(true);
        this.allPlayers[i].setWasOffsideWhenPassed(true);
        if (false && this.expectedPlayers[i].isMe()) {
          System.out
              .println(this.world.getCycleAndPlayerString() + " I WAS OFFSIDE");
        }
      }
      else {
        this.expectedPlayers[i].setWasOffsideWhenPassed(false);
        this.allPlayers[i].setWasOffsideWhenPassed(false);
      }
    }
  }

  /**
   * removes the offsideWhenPassed flag for all players
   * 
   */
  private void removePlayersOffside() {

    for (int i = 0; i < 11; i++) {
      this.expectedPlayers[i].setWasOffsideWhenPassed(false);
      this.allPlayers[i].setWasOffsideWhenPassed(false);
    }
  }

  /**
   * when teammates get near the ball they can't have been offside before
   * (called after a VI)
   */
  private void correctOffsidePlayers() {

    // opponent can kick -> no offside
    if (this.world.ballCanBeKickedByOpponent()) {
      for (int i = 0; i < 11; i++) {
        this.expectedPlayers[i].setWasOffsideWhenPassed(false);
        this.allPlayers[i].setWasOffsideWhenPassed(false);
      }
      return;
    }

    // player near ball -> no offside
    for (int i = 0; i < 11; i++) {
      if (this.world.getPlayMode() != PLAY_MODE.PLAY_ON || (this.expectedPlayers[i]
          .wasOffsideWhenPassed() && this.world.getBallRef().getAge(
          this.world.getBodyCycle(),
          robocup.component.worldobjects.Ball.UPDATE_MODE.SEEN_FULL) == 0 && this.expectedPlayers[i]
          .getDistance(this.world.getBallRef()) < 4.0)) {
        this.expectedPlayers[i].setWasOffsideWhenPassed(false);
        this.allPlayers[i].setWasOffsideWhenPassed(false);
      }
    }
  }

  /**
   * This method updates the player-model due to informations received from the
   * coach/trainer!
   * 
   * @param csi -
   *          a coach-say information!
   */
  public void update(final CoachSayInfo csi) {

    final MessageFactory mf = this.world.getMessageFactory();
    final LinkedList<MESSAGES> types = mf.getMessageTypes();

    for (int i = 0; i < types.size(); i++) {
      final MESSAGES m = types.get(i);

      switch (m) {
        case TRAINER_PLAYER_POS:
          mf.decodeMessageNumber(i);
          this.updatePlayerPosFromCSI(mf.getNumber(), mf.getAge(), mf
              .getPosition());
          break;

        case TRAINER_PLAYER_SPD:
          mf.decodeMessageNumber(i);
          this.updatePlayerSpdFromCSI(mf.getNumber(), mf.getAge(), mf
              .getSpeed());
          break;

        case KILL_PLAYER:
          mf.decodeMessageNumber(i);
          this.updatePlayerExistsFromCSI(mf.getNumber());
          break;

        case PLAYER_POS:
          mf.decodeMessageNumber(i);
          this.updatePlayerPosFromCSI(mf.getNumber(), mf.getAge(), mf
              .getPosition());
          break;

        case GOALIE_POS:
          mf.decodeMessageNumber(i);
          this.updateGoaliePosFromCSI(mf.getNumber(), mf.getAge(), mf
              .getPosition());
          break;
        case TRAINER_OPPTYPES:
          mf.decodeMessageNumber(i);
          this.updatePlayerTypesFromCSI(mf.getIntArray());
      }
    }
  }

  /**
   * This method updates a player position after receiving a message from the
   * coach.
   */
  private void updatePlayerPosFromCSI(final int num,
      final int age,
      final Vektor pos) {

    this.allPlayers[num - 1].getPosition().copy(pos);
    this.allPlayers[num - 1].setBodyCycle(this.world.getBodyCycle());
    this.allPlayers[num - 1].setReliable(true);
    this.allPlayers[num - 1].setCycle(this.world.getCycle());
    this.expectedPlayers[num - 1].copy(this.allPlayers[num - 1]);

  }

  /**
   * Sets the player configs to the players as communicated by the coach.
   * 
   * @param intArray -
   *          an array containg all the player type indixes
   */
  private void updatePlayerTypesFromCSI(final int[] intArray) {
    System.out.println("Status Changed!");
    final Player[] opps = this.getAllPlayers();
    final PConf[] avail = this.world.getAvailablePconfs();
    for (int i = 11; i < opps.length; i++) {
      opps[i].setPConf(avail[intArray[i-11]]);
      opps[i].setStatusChanged(true);
    }
//    final Player[] expected = this.getExpectedPlayers();
//    for (int i = 11; i < expected.length; i++) {
//      expected[i].setPConf(avail[intArray[i]]);
//      expected[i].setStatusChanged(true);
//    }
    
  }

  /**
   * This method updates a goalie position after receiving a message from
   * another player or the coach.
   */
  private void updateGoaliePosFromCSI(final int num,
      final int age,
      final Vektor pos) {

    this.allPlayers[num - 1].getPosition().copy(pos);
    this.allPlayers[num - 1].setBodyCycle(this.world.getBodyCycle());
    this.allPlayers[num - 1].setReliable(true);
    this.allPlayers[num - 1].setCycle(this.world.getCycle());
    this.allPlayers[num - 1].setGoalie(true);
    this.expectedPlayers[num - 1].copy(this.allPlayers[num - 1]);

  }

  /**
   * This method updates a player position after receiving a message from
   * another player or the coach.
   */
  private void updatePlayerSpdFromCSI(final int num,
      final int age,
      final Vektor spd) {

    // update for myself:
    this.allPlayers[num - 1].getSpeed().copy(spd);
    this.allPlayers[num - 1].setBodyCycle(this.world.getBodyCycle());
    this.allPlayers[num - 1].setReliable(true);
    this.allPlayers[num - 1].setSpeedReliable(true);
    this.allPlayers[num - 1].setBodyCycle(this.world.getBodyCycle() - 1 + age);
    this.allPlayers[num - 1].setCycle(this.world.getCycle() - 1 + age);
    this.expectedPlayers[num - 1].copy(this.allPlayers[num - 1]);
  }

  /**
   * This metod kills a player from world-model! Only used in training sessions!
   * 
   * @param num
   *          the players number
   */
  private void updatePlayerExistsFromCSI(final int num) {

    this.allPlayers[num - 1].setExists(false);
    this.expectedPlayers[num - 1].copy(this.allPlayers[num - 1]);
  }

  /**
   * Initializes the arrays of players.
   * 
   */
  public final void initPlayerArrays() {

    for (int i = 0; i < 11; i++) {
      if (i == 0) {
        // Goalie
        // if goalie doesn't have number one it get's overwritten when
        // first seening number 1
        // (see updatePlayerAfterVI)
        this.teammates[0] = new Player(-1, this.world.getBodyCycle(), 1,
            new Vektor(), new Vektor(), 0, 0, 8000.0, 1.0, 1.0, false, true,
            true, new PConf());
        this.opponents[0] = new Player(-1, this.world.getBodyCycle(), 1,
            new Vektor(Vektor.XY, 0, 50), new Vektor(), 0, 0, 8000.0, 1.0, 1.0,
            false, false, false, new PConf());
      }
      else {
        this.teammates[i] = new Player(-1, this.world.getBodyCycle(), i + 1,
            new Vektor(), new Vektor(), 0, 0, 8000.0, 1.0, 1.0, false, true,
            false, new PConf());
        // set all opponents to a point ;-) until they are seen or communicated
        // TODO improve rw
        final PConf oppstandard = new PConf();
        oppstandard.DASH_POWER_RATE = 0.0075;
        oppstandard.setId(7);
        this.opponents[i] = new Player(-1, this.world.getBodyCycle(), i + 1,
            new Vektor(10, 0), new Vektor(), 0, 0, 8000.0, 1.0, 1.0, false,
            false, false, oppstandard);
      }
      this.opponents[i].setReliable(false);

      // connect allPlayers to players (arrays contain references to same
      // player objects)
      this.allPlayers[i] = this.teammates[i];
      this.allPlayers[i + 11] = this.opponents[i];

      this.expectedPlayers[i] = this.teammates[i].cloned();
      this.expectedPlayers[i + 11] = this.opponents[i].cloned();
      this.expectedTeammates[i] = this.expectedPlayers[i];
      this.expectedOpponents[i] = this.expectedPlayers[i + 11];

      this.nextPlayers[i] = this.teammates[i].cloned();
      this.nextPlayers[i + 11] = this.opponents[i].cloned();
    }

    this.setTeammatesBeforeKickOff();
  }

  /**
   * This methods actualizes the references of the self-player inside the arrays
   * (teammates, allplayers) to the given one
   * 
   * @param self -
   *          the new reference to self
   */
  public void initSelf(final Player self) {

    this.allPlayers[self.getNumber() - 1] = self;
    this.teammates[self.getNumber() - 1] = self;
  }

  /**
   * getClosestOpponentTo
   * 
   * @param v
   *          Position Vektor
   * @return Player - opponent with shortest distance to the Vektor
   *         <code>v</code> or null if there are no opponents in the player's
   *         current world model
   */
  public final Player getClosestOpponentTo(final Vektor v) {

    return this.getClosestPlayerTo(this.opponents, v, false);
  } // end method getClosestOpponentTo(Vektor)

  /**
   * This method returns a <code>LinkedList</code> of all known opponents that
   * are at the given <code>angle</code> plus/minus <code>deviation</code>
   * and within the given <code>distance</code>. Note that if you give a
   * <code>deviation</code> of 180, you'll get all opponents within a circle
   * around the player with the radius <code>
   * maxDistance</code>.
   * 
   * @param angle -
   *          cone angle
   * @param deviation -
   *          half cone width
   * @param maxDistance -
   *          cone length
   * @return LinkedList of <code>Player</code>s inside the cone
   */
  public final LinkedList<Player> getOpponents(final Vektor position,
      final double angle,
      final double deviation,
      final double maxDistance) {

    final LinkedList<Player> enemies = new LinkedList<Player>();
    for (final Player opp : this.opponents) {
      // TODO define reliability (rw)
      if (opp.isReliable()) {
        final double angleToOpp = position.getAngleTo(opp.getPosition());
        if (this.world.getDistance(opp) <= maxDistance && Math.abs(Vektor
            .normalize(angleToOpp - angle)) <= deviation) {
          enemies.add(opp.cloned());
        }
      }
    }
    return enemies;
  }

  /**
   * calculates the player with the shortest distance to a position.
   * 
   * @param players -
   *          array of Player(s)
   * @param pos -
   *          absolute position
   * @param allowMe -
   *          include the player himself
   * 
   * @return Player - player out of <code>players</code> in shortest distance
   *         to <code>pos</code>
   */
  public final Player getClosestPlayerTo(final Player[] players,
      final Vektor pos,
      boolean allowMe) {

    Player nearestPlayer = null, tempPlayer;
    double minDistance = 999.99, tempDistance;

    for (int i = 0; i < players.length; i++) {
      if (!players[i].isReliable() || (players[i].isMe() && !allowMe)) {
        ;
      }
      else {
        tempPlayer = players[i];
        tempDistance = pos.getDistance(tempPlayer.getPosition());
        if (tempDistance < minDistance) {
          nearestPlayer = tempPlayer;
          minDistance = tempDistance;
        } // end if
      }
    } // end for
    return nearestPlayer;
  }

  /**
   * returns the y-coordinate off the estimated offside line (offensive mode).
   * the offside-y value is the coordinate of the opponent with the 2nd highest
   * y-position-value. (As in normal soccer.)
   * 
   * @return y coordinate of offside line
   */
  public double getTeamOffsideY() {

    // was calculated before?
    final Double y = this.world.getShortTermMemory().get(
        ShortTermMemory.DOUBLES.TEAM_OFFSIDE_Y);
    if (y != null) {
      return y;
    }

    // ball is always an offside limit
    double maxY = Math.max(this.world.getBallRef().getPosition().y, 0);
    double maxY2nd = maxY;
    double tmpY = maxY;

    // TODO remember these Players in ShortTermMemory ?!(rw)
    Player lastOpp = null;
    @SuppressWarnings("unused")
    Player lastOpp2nd = null;

    // for all opponents except goalie, find closest to opponents goal line
    // as this opponent marks the offside line
    for (final Player p : this.opponents) {
      if (p.isReliable()
      // && this.world.inField(p.getPosition())
      ) {

        // saving some time
        if (maxY2nd - p.getPosition().y > 3) {
          continue;
        }

        // the opponent might have dashed in the meantime
        // this is only of importance if he dashes towards our goal
        // otherwise we only underestimate the offside-y
        final boolean turnForAttacking = Math.abs(p.getBodyDir()) > 120;
        if (turnForAttacking) {
          // might have dashed 1x
          if (p.getAge(this.world.getBodyCycle(), UPDATE_MODE.SEEN_NOTEAM) == 1) {
            tmpY = p.getPosition().y + Math.cos(Math.toRadians(p.getBodyDir())) * 1.0;
          }
          // might have dashed more than 1x
          // as we don't know how fast the opponent is moving we have
          // to estimate his vel.
          else {
            tmpY = p.getPosition().y - p.getAge(this.world.getBodyCycle(),
                UPDATE_MODE.SEEN_NOTEAM) * p.getPConf().PLAYER_SPEED_MAX * 0.7;
          }
        }
        // 2 cycles for turning, afterwards same as above
        else {
          tmpY = p.getPosition().y - Math.max(p.getAge(this.world
              .getBodyCycle(), UPDATE_MODE.META_BASED_ON_INFO) - 2, 0) * p
              .getPConf().PLAYER_SPEED_MAX * 0.5;
        }

        if (tmpY > maxY) {
          lastOpp2nd = lastOpp;
          lastOpp = p;
          maxY2nd = maxY;
          maxY = tmpY;
        }
        else if (tmpY > maxY2nd) {
          maxY2nd = tmpY;
        }
      }
    }

    // as we don't know everything about the world, use other players
    // knowledge!
    // a teammate that is turned towards the opp. goal consideres himself
    // in an non offside position -> use this information
    for (final Player p : this.teammates) {
      if (!p.isMe() && p.getAge(this.world.getBodyCycle(),
          UPDATE_MODE.SEEN_NOTEAM) < 3 && p.getPosition().y - p.getAge(
          this.world.getBodyCycle(), UPDATE_MODE.SEEN_NOTEAM) > maxY2nd && Math
          .abs(p.getBodyDir()) < 70.0) {
        maxY2nd = p.getPosition().y;
      }
    }

    this.world.getShortTermMemory().set(ShortTermMemory.DOUBLES.TEAM_OFFSIDE_Y,
        maxY2nd);
    return maxY2nd;
  }

  /**
   * the y-coordinate of the offside line when we are in defense. by now it's
   * the position.y of the field player (teammate) nearest to our goalline.
   * 
   * @return
   */
  public double getOpponentOffsideY() {

    // was calculated before?
    final Double y = this.world.getShortTermMemory().get(
        ShortTermMemory.DOUBLES.OPPONENT_OFFSIDE_Y);
    if (y != null) {
      return y;
    }

    double minY = this.world.getBallRef().getPosition().y;

    for (final Player p : this.teammates) {
      if (!p.isGoalie() && this.world.inField(p.getPosition())) {
        minY = Math.min(p.getPosition().y, minY);
      }
    }

    this.world.getShortTermMemory().set(
        ShortTermMemory.DOUBLES.OPPONENT_OFFSIDE_Y, minY);
    return minY;
  }

  /**
   * @return Returns the allPlayers (reference!!).
   */
  public Player[] getAllPlayers() {

    return this.allPlayers;
  }

  /**
   * @return Returns the opponents (reference!!).
   */
  public Player[] getOpponents() {

    return this.opponents;
  }

  /**
   * @return Returns the teammates (reference!!).
   * 
   */
  public Player[] getTeammates() {

    return this.teammates;
  }

  /**
   * All the players that have the ball in their kickable range. (self is
   * included!)
   * 
   * @param b -
   *          ball
   * @param player -
   *          intersting players
   * @return list of all relevant players
   */
  public LinkedList<Player> getPlayersThatCanKick(final Ball b,
      final Player[] player) {

    final LinkedList<Player> kicker = new LinkedList<Player>();
    for (final Player p : player) {

      if (p.isReliable() && b.getDistance(p) < p.getKickDistance()) {
        kicker.add(p);
      }
    }
    return kicker;
  }

  /**
   * @return Returns the expectedPlayers.
   */
  public Player[] getExpectedPlayers() {

    return this.expectedPlayers;
  }

  /**
   * @return Returns the nextPlayers (reference!!).
   */
  public Player[] getNextPlayers() {

    return this.nextPlayers;
  }

  /**
   * the opponents goalie.
   * 
   * @return the opponents goalie; null if unknown
   */
  public Player getOpponentsGoalie() {

    if (this.numberGoalieOpponent > 0) {
      return this.opponents[this.numberGoalieOpponent - 1];
    }

    return null;
  }

  /**
   * prints the distance matrix.
   * 
   * @param infos -
   *          player infos which lead to the matrix
   */
  private void printMatchMatrix(final LinkedList<PlayerInfo> infos) {

    System.out
        .println(this.world.getCycleAndPlayerString() + " MatchDistances:");
    for (final Player p : this.teammates) {
      System.out.println(p + " Age:" + this.world.getAge(p));
    }
    for (int rowIndex = 0; rowIndex < this.matchDistancesRowCount + 1; rowIndex++) {
      if (rowIndex == 0 && infos != null) {
        System.out.print(new Vektor());
      }
      else if (infos != null) {
        System.out.print(infos.get(rowIndex - 1).getAbsPlayerPosition());
      }
      for (int columnIndex = 0; columnIndex < this.matchDistancesColumnCount; columnIndex++) {

        System.out.print("\t" + RCMath.round(
            this.matchDistances[rowIndex][columnIndex], 2));
      }
      System.out.println();
    }
  }

  /**
   * gets an array of sorted players from the memory and updates the sorting if
   * necessary.
   * 
   * @param sorted -
   *          the sort option
   * @param position -
   *          the position (or null)
   * @return - an array with the sorted players
   */
  public Player[] getPlayersSortedBy(final SORTED sorted,
      final Vektor position) {

    final int timeStamp = this.world.getBodyCycle() * 10 + (this.world.isViBased() ? 1 : 0);

    if (this.sortingTimes.get(sorted) == null || this.sortingTimes.get(sorted) < timeStamp || position != null) {

      this.playerComparator.setDimension(sorted.getDimension());
      this.playerComparator.setOrder(sorted.getOrder());
      this.playerComparator.setPosition(position);

      Arrays.sort(this.sortedPlayersMemory.get(sorted), this.playerComparator);
      this.sortingTimes.put(sorted, timeStamp);
    }

    return this.sortedPlayersMemory.get(sorted);
  }

  /**
   * finds out on what side of the field most oponents are. this is important if
   * we want the opponents to run by passing to the opposite side.
   * 
   */
  private void setDominatedSide() {

    double sumX = 0;
    double counter = 0;

    for (final Player opp : this.expectedOpponents) {
      if (opp.isReliable() && !opp.isGoalie() && opp.getPosition().y > this.world
          .getBallRef().getPosition().y - 15) {
        sumX += opp.getPosition().x;
        counter++;
      }
    }

    final double avgX = counter == 0 ? 0 : sumX / counter;

    if (avgX > 7) {
      this.dominatedSide = DOMINATED_SIDE.RIGHT;
    }
    else if (avgX < -7) {
      this.dominatedSide = DOMINATED_SIDE.LEFT;
    }
    else {
      this.dominatedSide = DOMINATED_SIDE.NONE;
    }

    if (false && this.world.getNumber() == 10 && this.world.getCycle() % 20 == 0) {
      System.out
          .println(this.world.getCycleAndPlayerString() + " " + this.dominatedSide
              .toString());
    }
  }

  /**
   * This method sorts the given list of players by the specified criteria!
   * 
   * @param players -
   *          the list of players to sort
   * @param dim -
   *          the sorting type
   * @param order -
   *          the order to sort (ascending/descending)
   * @param pos -
   *          the position to compare to (only for dim == DISTANCE)
   */
  public void sortPlayersBy(final LinkedList<Player> players,
      final DIMENSION dim,
      final ORDER order,
      final Vektor pos) {

    this.playerComparator.setOrder(order);
    switch (dim) {
      case DISTANCE:
        this.playerComparator.setPosition(pos);
        this.playerComparator.setDimension(dim);
        break;
      case X_AXIS:
        this.playerComparator.setDimension(dim);
        break;
      case Y_AXIS:
        this.playerComparator.setDimension(dim);
        break;
    }
    // sort the list
    Collections.sort(players, this.playerComparator);
  }

  /**
   * true if both players are neighbours
   * 
   * @param p1
   * @param p2
   * @return
   */
  public boolean isNeighbour(final Player p1,
      final Player p2) {

    return this.neighbours[p1.getRoleNumber() - 1][p2.getRoleNumber() - 1];
  }

  /**
   * true if both players are direct neighbours
   * 
   * @param p1
   * @param p2
   * @return
   */
  public boolean isDirectNeighbour(final Player p1,
      final Player p2) {

    return this.directNeighbours[p1.getRoleNumber() - 1][p2.getRoleNumber() - 1];
  }

  /**
   * @return Returns the dominatedSide.
   */
  public DOMINATED_SIDE getDominatedSide() {

    return this.dominatedSide;
  }

  /* (non-Javadoc)
   * @see robocup.component.infotypes.InfoReceiver#update(robocup.component.infotypes.PlayerTypesInfo)
   */
  public void update(final PlayerTypesInfo info) {

    // TODO Auto-generated method stub
    
  }

}
